﻿namespace RLab.Visualization

open System
open System.Drawing
open System.Windows.Forms
open Microsoft.DirectX
open Microsoft.DirectX.Direct3D

open GeometryDefinitions

//robot element
type Element = 
    { Model  : Mesh
      Offset : Vector3 //offset used for setting model in global coordinate origin  (?)there should be assuming that first element join lies in (0,0,0)
      Joint  : Vector3 //constant join position depending on local coordinate system
      Move   : float32->Matrix
      Step   : float32
      Range  : float32*float32 } //funkcja ruchu złącza( 1 stopień swobody dla każdego przegubu )

[<CompilationRepresentation(CompilationRepresentationFlags.ModuleSuffix)>]
module Element =
    let elementMatrices ( els : ( Element * float32 ) list ) =     
        let computeLocalCoords (robo : (Element*float32) list) = 
            let rec getCoords r l result=
                match r,l with
                | (e,d)::t,lo -> 
                    let lnext = 
                        //TODO:extract rotation from joint transformation matrix
                        //rotationMatrix (e.Move d),e.Joint + translationVector(e.Move d)
                        (e.Move d),e.Joint + translationVector(e.Move d)
                    getCoords t lnext (List.append result  [l,e])
                | [], _ -> result
            getCoords robo (Matrix.Identity,O) []
        let localCoords = computeLocalCoords els
        let rec rectransformations
            ( l:( (Matrix*Vector3)*Element )  list) 
            ( prevMatrix : Matrix )  
            (prevLocal : Matrix*Matrix) 
            (prevTranf : Vector3->Matrix)
            result : ( Element*Matrix ) list = 
            match l with
            | h::t -> 
                let rotation = (fst prevLocal) * fst (fst h)
                let tranf v = prevTranf ( prevMatrix @* ( snd (fst h) + v) )
                let local = rotation,tranf O
                rectransformations 
                    t ( fst (fst h) ) 
                    local tranf 
                     [ (snd h),(fst local)*(snd local)] @result
            | [] -> result
        let crds = 
            rectransformations
                localCoords 
                Matrix.Identity 
                (Matrix.Identity,Matrix.Identity) 
                (fun v ->  Matrix.Translation(v))
                []
        crds
    let validateMovement element m = 
        match element.Range,m with
        | (min,max),m when min>max -> failwith "Invalid element definition contraints."
        | (min,max),m when min > m || max < m -> failwith "Joint movement value out of range."
        | _ -> ()

open System.Threading

type Robot( renderer : Renderer, elements : List<_> ) as r =   
    let mutable els = seq{ for e in elements -> ( e,Matrix.Identity ) }
    let mutable jointPositions = seq{ for e in elements -> 0.0f }    

    do  Seq.map ( fun e -> 0.0f ) elements |> r.PutJoints
    do renderer.DrawScene.Add( 
        fun d ->
            els |> Seq.iter(
                fun (e,m) ->
                     d.SetTransform( TransformType.World,m 
                     * Matrix.Scaling(0.3f,0.3f,0.3f ) ) 
                     e.Model.DrawSubset(0) ) )                         
    member r.PutJoints(movements) = 
        jointPositions <-  movements
        els <- 
            elements |> 
            Seq.map2 (fun m e -> (e,m)) movements |> 
            List.ofSeq |> Element.elementMatrices
    member r.Posistions = 
        jointPositions
    member r.MoveJoints(movements) = 
        let fsign x = if abs x <> x then -1.0f else 1.0f 
        let next() = 
            let maps =
                Seq.map2 (fun j m -> (j,m) ) jointPositions movements |>
                Seq.map2 (fun e (j,m) -> Element.validateMovement e m; (j,m,e.Step) ) elements
            seq{for (j,m,s) in maps->
                    match j,m,s with
                    | _ when abs (j - m) > s -> j+s*fsign(m-j),true
                    | _ -> j,false}        
        let rec moveNTimes nextval = 
                if Seq.exists (fun (m,b) -> b) nextval then                    
                    nextval |> Seq.map (fun (m,b) -> m) |> r.PutJoints
                    Thread.Sleep(10)
                    moveNTimes (List.ofSeq (next()))
        moveNTimes (List.ofSeq (next()))
    member r.GoBase() =        
        r.MoveJoints([0.0f;0.0f;0.0f;0.0f])
        
    

    